/*
 * enc_rawsock.c
 *
 *  Created on: 2018年10月16日
 *  Author: Degang.Wu (windragon@126.com)
 *  Desc:
 */


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <linux/if_ether.h>
#include <linux/if_packet.h>
//#include <linux/in.h>
#include <netinet/in.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <errno.h>
#include <sys/time.h>
#include <time.h>
#include <bits/byteswap.h>		// to use  bw_htons (x);
#include "public.h"
#include "enc_shrm.h"
#include "enc_type.h"

#define _RAW_SOCK_SECOND_LEVEL	0

U16 bw_htons (U16 x)
{
#if BYTE_ORDER == BIG_ENDIAN
  return x;
#elif BYTE_ORDER == LITTLE_ENDIAN
  return __bswap_16 (x);
#else
# error "What kind of system is this?"
#endif
}

U32 bw_htonl (U32 x)
{
#if BYTE_ORDER == BIG_ENDIAN
  return x;
#elif BYTE_ORDER == LITTLE_ENDIAN
  return __bswap_32 (x);
#else
# error "What kind of system is this?"
#endif
}

U32	enc_get_localaddr(const char* ethname)
{
    int sock;
    int ret;
    struct ifreq ifr;

    sock = socket(AF_INET, SOCK_STREAM, 0);
    if (sock < 0)
    {
    	return 0;
    }
    strcpy(ifr.ifr_name, ethname);
    ret = ioctl(sock, SIOCGIFADDR, &ifr);
    SAFE_CLOSE_FD(sock);

    if (ret < 0)
    {
    	return 0;
    }

    return ((struct sockaddr_in*)&ifr.ifr_addr)->sin_addr.s_addr;

}

int enc_get_mac(const char* ethname, char mac[6])
{
	struct ifreq ifr;
	int i = 0;

	int sock = socket(AF_INET, SOCK_STREAM, 0);

	strcpy(ifr.ifr_name, ethname);
	int ret = ioctl(sock, SIOCGIFHWADDR, &ifr);
	if (ret < 0)
	{
		BW_INFO("ioctl[SIOCGIFHWADDR] failed\n");
		SAFE_CLOSE_FD(sock);
		return -1;
	}

	char mac_s[32];
	memset(mac_s, 0, sizeof(mac_s));
	for (i = 0; i < 6; i++)
	{
		mac[i] = ifr.ifr_hwaddr.sa_data[i];
		snprintf(mac_s+strlen(mac_s), sizeof(mac_s)-strlen(mac_s), "%02x ", mac[i] & 0xff);
	}

	BW_INFO("GET `%s` MAC:%s\n", ethname, mac_s);
	SAFE_CLOSE_FD(sock);

	return 0;
}

int enc_single_addr_to_mac(unsigned int ipaddr, const char* ethname, char mac[6])
{
	BW_INFO("single ipaddr=0x%x\n", ipaddr);
	int sock_raw_fd = socket(PF_PACKET, SOCK_RAW, bw_htons(ETH_P_ALL));
	memset(mac, 0xff, 6);		//init all "1"
	if (sock_raw_fd < 0)
	{
		return -1;
	}

	st_arp_pkt 	pkt;
	memset(&pkt, 0, sizeof(st_arp_pkt));
	ether* eth = &pkt.ethhdr;

	// ethernet src mac
	if (enc_get_mac(ethname, eth->smac) < 0)
	{
		return -1;
	}
	// ethernet dst mac
	memset(eth->dmac, 0xff, 6);
	// arp type : 0x0806
//	eth->eth_typ_len[0] = 0x08;
//	eth->eth_typ_len[1] = 0x06;
	eth->eth_type = bw_htons(ETH_P_ARP);

	// arp head
	arp_hdr* arphdr = &pkt.arphdr;
	arphdr->htype = bw_htons(1);
	arphdr->ptype = bw_htons(ETH_P_IP);
	arphdr->hlen = 6;
	arphdr->plen = 4;
	arphdr->opcode = bw_htons(ARPOP_REQUEST);
	memcpy(arphdr->sender_mac, eth->smac, 6);
	memset(arphdr->target_mac, 0, 6);
	arphdr->sender_ip = enc_get_localaddr(ethname);
	arphdr->target_ip = ipaddr;

	struct sockaddr_ll sll;					//原始套接字地址结构
	struct ifreq req;					//网络接口地址
	strncpy(req.ifr_name, ethname, IFNAMSIZ);	//指定网卡名称

	ioctl(sock_raw_fd, SIOCGIFINDEX, &req);
	bzero(&sll, sizeof(sll));
	sll.sll_ifindex = req.ifr_ifindex;

	int len = sendto(sock_raw_fd, &pkt, sizeof(pkt), 0 , (struct sockaddr *)&sll, sizeof(sll));
	if(len == -1)
	{
		SAFE_CLOSE_FD(sock_raw_fd);
		BW_INFO("send arp failed\n");
	}

	// recv arp ack
	unsigned char recv_msg[1024] = {0};
	recvfrom(sock_raw_fd, recv_msg, sizeof(recv_msg), 0, NULL, NULL);
	if (recv_msg[21] == ARPOP_RESPONSE)			//ARP应答
	{
		char resp_mac[18] = "";		//arp响应的MAC
		char resp_ip[16] = "";		//arp响应的IP

		sprintf(resp_mac, "%02x:%02x:%02x:%02x:%02x:%02x", recv_msg[22],
				recv_msg[23], recv_msg[24], recv_msg[25], recv_msg[26],
				recv_msg[27]);
		sprintf(resp_ip, "%d.%d.%d.%d", recv_msg[28], recv_msg[29],
				recv_msg[30], recv_msg[31]);
		BW_INFO("IP:%s - MAC:%s\n", resp_ip, resp_mac);

		memcpy(mac, &recv_msg[22], 6);
	}

	SAFE_CLOSE_FD(sock_raw_fd);
	return 0;
}

int enc_multiaddr_to_mac(unsigned int ipaddr, char mac[6])
{
	BW_INFO("multi ipaddr=0x%x\n", ipaddr);
	if ((ipaddr & 0x000000e0) != 0x000000e0)
	{
		// not multiaddr
		memset(mac, 0xff, 6);		// use broadcast
		return -1;
	}

	memset(mac, 0, 6);
	mac[0] = 0x01;
	mac[1] = 0x00;
	mac[2] = 0x5e;

	unsigned char* paddr = (unsigned char*)&ipaddr;
	unsigned char high, low;
	high = paddr[1] >> 4;	// 16
	low = paddr[1] % 16;
	mac[3] = (high & 0x07) << 4 | low ;

	high = paddr[2] >> 4;
	low = paddr[2] % 16;
	mac[4] = high << 4 | low ;

	high = paddr[3] >> 4;
	low = paddr[3] % 16;
	mac[5] = high << 4 | low ;

	char mac_s[32];
	memset(mac_s, 0, sizeof(mac_s));
	int i = 0;
	for (i = 0; i < 6; i++)
	{
		snprintf(mac_s+strlen(mac_s), sizeof(mac_s)-strlen(mac_s), "%02x ", mac[i] & 0xff);
	}

	BW_INFO("GET multi `0x%x` MAC:%s\n", ipaddr, mac_s);

	return 0;
}

//caculate ip checksum
unsigned short int enc_ipcsum (unsigned short int *addr, int len)
{
	int nleft = len;
	unsigned int sum = 0;
	unsigned short int *w = addr;
	unsigned short int answer = 0;
	while (nleft > 1)
	{
		sum += *w++;
		nleft -= sizeof(unsigned short int);
	}

	if (nleft == 1)
	{
		*(char *) (&answer) = *(char *) w;
		sum += answer;
	}

	sum = (sum >> 16) + (sum & 0xFFFF);
	sum += (sum >> 16);
	answer = ~sum;

	return (answer);
}

unsigned short int enc_udp_csum(ipheader iphdr, udpheader udphdr,
		unsigned char *payload, int payloadlen)
{
	char buf[65535];
	char *ptr;
	int chksumlen = 0;
	int i;

	ptr = &buf[0];  // ptr points to beginning of buffer buf

	/****************pseudo header  12byte*********************************/

	// Copy source IP address into buf (32 bits)
	memcpy(ptr, &iphdr.iph_sourceip, sizeof(iphdr.iph_sourceip));
	ptr += sizeof(iphdr.iph_sourceip);
	chksumlen += sizeof(iphdr.iph_sourceip);

	// Copy destination IP address into buf (32 bits)
	memcpy(ptr, &iphdr.iph_destip, sizeof(iphdr.iph_destip));
	ptr += sizeof(iphdr.iph_destip);
	chksumlen += sizeof(iphdr.iph_destip);

	// Copy zero field to buf (8 bits)
	*ptr = 0;
	ptr++;
	chksumlen += 1;

	// Copy transport layer protocol to buf (8 bits)
	memcpy(ptr, &iphdr.iph_protocol, sizeof(iphdr.iph_protocol));
	ptr += sizeof(iphdr.iph_protocol);
	chksumlen += sizeof(iphdr.iph_protocol);

	// Copy UDP length to buf (16 bits)
	memcpy(ptr, &udphdr.udph_len, sizeof(udphdr.udph_len));
	ptr += sizeof(udphdr.udph_len);
	chksumlen += sizeof(udphdr.udph_len);

	/*******************end pseudo header*************************/

	// Copy UDP source port to buf (16 bits)
	memcpy(ptr, &udphdr.udph_srcport, sizeof(udphdr.udph_srcport));
	ptr += sizeof(udphdr.udph_srcport);
	chksumlen += sizeof(udphdr.udph_srcport);

	// Copy UDP destination port to buf (16 bits)
	memcpy(ptr, &udphdr.udph_destport, sizeof(udphdr.udph_destport));
	ptr += sizeof(udphdr.udph_destport);
	chksumlen += sizeof(udphdr.udph_destport);

	// Copy UDP length again to buf (16 bits)
	memcpy(ptr, &udphdr.udph_len, sizeof(udphdr.udph_len));
	ptr += sizeof(udphdr.udph_len);
	chksumlen += sizeof(udphdr.udph_len);

	// Copy UDP checksum to buf (16 bits)
	// Zero, since we don't know it yet
	*ptr = 0;
	ptr++;
	*ptr = 0;
	ptr++;
	chksumlen += 2;

	// Copy payload to buf
	memcpy(ptr, payload, payloadlen);
	ptr += payloadlen;
	chksumlen += payloadlen;

	// Pad to the next 16-bit boundary
	for (i = 0; i < payloadlen % 2; i++, ptr++)
	{
		*ptr = 0;
		ptr++;
		chksumlen++;
	}

	return enc_ipcsum((unsigned short int *) buf, chksumlen);
}

// ADD BY degang.wu, 20180926
// use raw socket, so control src ip and src port of output stream
int enc_open_sndsocket_raw(stENC_ETHIOSET* piodesc, stENC_RAW_SOCK_SET* prawst)
{
	char localname[20];
	char sipaddr_s[50];
	char dipaddr_s[50];
	unsigned int ethid = piodesc->ethio_ethid;
	unsigned int sipaddr = piodesc->ethio_sipaddr;
	unsigned int dipaddr = piodesc->ethio_dipaddr;

	snprintf(sipaddr_s, sizeof(sipaddr_s), "%u.%u.%u.%u", (sipaddr >> 24),
			(sipaddr >> 16) & 0xff, (sipaddr >> 8) & 0xff, (sipaddr & 0xff));
	snprintf(dipaddr_s, sizeof(dipaddr_s), "%u.%u.%u.%u", (dipaddr >> 24),
			(dipaddr >> 16) & 0xff, (dipaddr >> 8) & 0xff, (dipaddr & 0xff));

	memset(prawst, 0, sizeof(stENC_RAW_SOCK_SET));

//	ethid = ipdly_get_map_ethid(ethid);

	if (piodesc->ethio_isenable == 0 || piodesc->ethio_dipaddr == 0)
	{
		return -1;
	}

#ifdef _DEBUG_RAW_DEMO
	snprintf(localname, sizeof(localname), "eno33554960");
#else
	if (enc_is_master())
	{
		snprintf(localname, sizeof(localname), "%s",
				(ethid < g_encset_mst->enc_ethdevnum) ?
				(char*) g_encset_mst->enc_ethdevname[ethid] : "localhost");
	}
	else
	{
		snprintf(localname, sizeof(localname), "%s",
				(ethid < g_encset_slv->enc_ethdevnum) ?
				(char*) g_encset_slv->enc_ethdevname[ethid] : "localhost");
	}

#endif

	BW_INFO("===========>open raw snd socket, sipaddr = %s, sport = %d, dipaddr = %s, dport = %d, localeth = %s\n",
			sipaddr_s, piodesc->ethio_sipport, dipaddr_s, piodesc->ethio_dipport, localname);

	prawst->sipaddr = bw_htonl(piodesc->ethio_sipaddr);
	prawst->sipport = piodesc->ethio_sipport;
	prawst->dipaddr = bw_htonl(piodesc->ethio_dipaddr);
	prawst->dipport = piodesc->ethio_dipport;


	int ret = 0;
	ret = enc_get_mac(localname, BW_CHAR prawst->shwaddr);
	if (ret < 0)
	{
		BW_INFO("get src mac failed!\n");
		return -1;
	}

	if ((prawst->dipaddr & 0x000000e0) == 0x000000e0)
	{	// multi broadcast
		ret = enc_multiaddr_to_mac(prawst->dipaddr, BW_CHAR prawst->dhwaddr);
		if (ret < 0)
		{
			BW_INFO("get dst mac failed!\n");
			return -1;
		}
	}
	else
	{
		enc_single_addr_to_mac(prawst->dipaddr, localname, BW_CHAR prawst->dhwaddr);
	}


	// +++eth header
	ether* eth = &prawst->pkthdr.ethhdr;
	eth->eth_type = bw_htons(ETH_P_IP);
	memcpy(eth->smac, prawst->shwaddr, sizeof(eth->smac));
	memcpy(eth->dmac, prawst->dhwaddr, sizeof(eth->dmac));
	// ---end eth header

	memset(&prawst->device, 0, sizeof(prawst->device));
	if ((prawst->device.sll_ifindex = if_nametoindex(localname)) == 0)
	{
		BW_INFO("if_nametoindex() failed to obtain interface index\n");
		return -1;
	}
	prawst->device.sll_family = AF_PACKET;
	memcpy(prawst->device.sll_addr, prawst->dhwaddr, sizeof(prawst->dhwaddr));
	prawst->device.sll_halen = bw_htons(sizeof(prawst->dhwaddr));

	//udp header
	prawst->pkthdr.udphdr.udph_srcport = bw_htons(prawst->sipport);
	prawst->pkthdr.udphdr.udph_destport = bw_htons(prawst->dipport);

	ipheader* ip = &prawst->pkthdr.iphdr;

	/*****************fill some udp header*********************************/
	prawst->pkthdr.udphdr.udph_destport = bw_htons(prawst->dipport);
	prawst->pkthdr.udphdr.udph_srcport = bw_htons(prawst->sipport);
	/*******************end fill udp header**************************/

	/********************fill some ip header****************************/
	ip->iplv = 1 << 6 | 5;	//version v4 or v6 and head_len 5
			   	   	   	   	//ip首部长度以32bit为单位计算
	ip->iph_tos = 0; // Low delay
	ip->iph_ident = 0;//标示字段 唯一标示一个数据包
	ip->iph_offset = 0x0040;//16bit include offset and flag
	ip->iph_ttl = 255; // time to live
	ip->iph_protocol = 17; // UDP
	ip->iph_sourceip = prawst->sipaddr;
	ip->iph_destip = prawst->dipaddr;
    // Calculate the checksum for integrity
	/******************end fill ip header**********************/

#if _RAW_SOCK_SECOND_LEVEL
	int sock_fd = socket(AF_PACKET, SOCK_RAW, bw_htons(ETH_P_IP));
#else
	int sock_fd = socket(AF_PACKET, SOCK_RAW, bw_htons(ETH_P_IP));
#endif
	if (sock_fd >= 0)
	{
		prawst->init_flag = 1;
	}

	struct ifreq ifr;
	memset(&ifr, 0x00, sizeof(ifr));
	strcpy(ifr.ifr_name, localname);
	if((setsockopt(sock_fd, SOL_SOCKET, SO_BINDTODEVICE, &ifr, sizeof(ifr))) < 0)
	{
		perror("setsockopt() cannot bind.");
	}

	int bufsize = SZ_256K;
	if (setsockopt(sock_fd, SOL_SOCKET, SO_SNDBUF, &bufsize, sizeof(bufsize)) < 0)
	{
		perror("setsockopt() set sndbuf");
	}
	if (setsockopt(sock_fd, SOL_SOCKET, SO_RCVBUF, &bufsize, sizeof(bufsize)) < 0)
	{
		perror("setsockopt() set rcvbuf");
	}

	return sock_fd;
}

static int sendto_raw(int fd, U8* buf, int len, struct sockaddr* out_addr, size_t addr_len)
{
	int ret = 0;
	int sndcount = 0;
	while (1)
	{
		sndcount++;
		ret = sendto(fd, buf, len, 0, out_addr, addr_len);
		if (ret < 0) {
			if (errno == EINTR || errno == EAGAIN) {
				continue;
			}
			if (errno == ENOBUFS && sndcount < 2)
			{
				usleep(1000);	//FIXME is ok?
				continue;
			}
			return ret;
		}

		return ret;
	}

	return ret;
}
int enc_sendto_raw(int fd, stENC_RAW_SOCK_SET* prawst, int buflen)
{
	if (fd < 0 || !prawst->init_flag)
	{
		return -1;
	}

	int ret = 0;

	ipheader* ip = &prawst->pkthdr.iphdr;

	/*****************fill udp header*********************************/
	prawst->pkthdr.udphdr.udph_len = bw_htons(sizeof(udpheader) + buflen);
	prawst->pkthdr.udphdr.udph_chksum = enc_udp_csum(*ip, prawst->pkthdr.udphdr, (U8*)prawst->pkthdr.buf, buflen);
	/*******************end fill udp header**************************/

	/********************fill ip header****************************/
	ip->iph_len = bw_htons(sizeof(ipheader) + sizeof(udpheader) + buflen);
    // Calculate the checksum for integrity
	ip->iph_chksum = 0;
	ip->iph_chksum = enc_ipcsum((unsigned short int *)ip, (int)sizeof(ipheader));
	/******************end fill ip header**********************/

//	ether			ethhdr;
//	ipheader		iphdr;
//	udpheader		udphdr;

	int send_len = sizeof(ether) + sizeof(ipheader) + sizeof(udpheader) + buflen;
	ret = sendto_raw(fd, (U8*)&prawst->pkthdr, send_len, (struct sockaddr*)&prawst->device, sizeof(prawst->device));

	if (ret < 0)
	{
		BW_INFO("sendto_raw error, send_len=%d, ret=%d[%s]\n", send_len, ret, strerror(errno));
		perror("sendto");
	}

	return ret;
}


